from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import LogInfo


def generate_launch_description():
    # 定义所有桥接配置
    bridges = [
        # ugv1 joint_state: GZ → ROS
        {
            'gz_topic': '/world/default/model/ugv1/joint_state',
            'ros_topic': '/world/default/model/ugv1/joint_state',
            'gz_type': 'gz.msgs.Model',
            'ros_type': 'sensor_msgs/msg/JointState',
            'direction': 'GZ_TO_ROS'
        },
    ]

    # 为每个桥接创建一个 parameter_bridge 节点
    bridge_nodes = []
    for i, bridge in enumerate(bridges):
        node = Node(
            package='ros_gz_bridge',
            executable='parameter_bridge',
            name=f'ros_gz_bridge_{i}',
            arguments=[
                f"{bridge['gz_topic']}@{bridge['ros_type']}@{bridge['gz_type']}"
            ],
            output='screen',  # 可选：显示日志
            remappings=[
                (bridge['gz_topic'], bridge['ros_topic'])
            ] if bridge['direction'] == 'GZ_TO_ROS' else []
            # 注意：对于 ROS_TO_GZ，remap 不需要（因为 ROS 话题名就是 ros_topic）
        )
        bridge_nodes.append(node)

    return LaunchDescription([
        LogInfo(msg='Starting ROS-Gz bridges...'),
        *bridge_nodes
    ])